/*
 *  proximityTrajectories.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-12-03.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "proximityTrajectories.h"
#include "displayOpenCV.h"

extern double delT;
extern double calibration;

void circleProximity( SetPoint *setpoint, MaskObject ball, double TIMESTEP ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=delT*setpoint->trajectoryStep*setpoint->ballSpeed/setpoint->pathRadius;
    
    CvMat *errPos=cvCreateMat( 2, 1, CV_32F );
    CvMat *errVel=cvCreateMat( 2, 1, CV_32F );
    
    cvmSet( errPos, 0, 0, cvmGet(ball.state, 0, 0) - cvmGet(setpoint->setPointR, 0, 0) );
    cvmSet( errPos, 1, 0, cvmGet(ball.state, 2, 0) - cvmGet(setpoint->setPointR, 2, 0) );
    
    cvmSet( errVel, 0, 0, cvmGet(ball.state, 1, 0) - cvmGet(setpoint->setPointR, 1, 0) );
    cvmSet( errVel, 1, 0, cvmGet(ball.state, 3, 0) - cvmGet(setpoint->setPointR, 3, 0) );
    
    if(cvNorm(errPos) < .01 /* Closer than 1 cm*/ ){
        setpoint->trajectoryStep++;
        setPoint_x=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
        setPoint_y=setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
        setPoint_vx=-setpoint->ballSpeed*sin(tau);
        setPoint_vy=setpoint->ballSpeed*cos(tau);
        
        cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
        cvmSet( setpoint->setPoint, 1, 0, setPoint_vx/1*0 );
        cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
        cvmSet( setpoint->setPoint, 3, 0, setPoint_vy/1*0 );
    }
    else if(cvNorm(errPos) < .0003 /* Closer than 3 cm*/ ){
        setpoint->trajectoryStep++;
        setPoint_x=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
        setPoint_y=setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
        setPoint_vx=-setpoint->ballSpeed*sin(tau);
        setPoint_vy=setpoint->ballSpeed*cos(tau);
        
        cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
        cvmSet( setpoint->setPoint, 1, 0, setPoint_vx*(.03-cvNorm(errPos))/.03 );
        cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
        cvmSet( setpoint->setPoint, 3, 0, setPoint_vy*(.03-cvNorm(errPos))/.03 );
    }
    
    else{ // Too far from point. Use only coordinates as setpoint.
        setPoint_x=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
        setPoint_y=setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
        cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
        cvmSet( setpoint->setPoint, 1, 0, 0 );
        cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
        cvmSet( setpoint->setPoint, 3, 0, 0 );
    }
    
    cvAdd(setpoint->integrationPositionErr, errPos, setpoint->integrationPositionErr);
    cvAdd(setpoint->integrationVelocityErr, errVel, setpoint->integrationVelocityErr);\
            
            cvReleaseMat( &errPos );
    cvReleaseMat( &errVel );
    return;
}

void ellipseProximity( SetPoint *setpoint, double runTime ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=runTime*setpoint->ballSpeed/setpoint->pathRadius;
    cvZero( setpoint->integrationPositionErr );
    
    setPoint_x=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
    setPoint_y=0.5*setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
    setPoint_vx=-setpoint->pathRadius*sin(tau);
    setPoint_vy=setpoint->pathRadius*cos(tau);
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    return;
}

void spiralProximity( SetPoint *setpoint, double runTime ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=runTime*setpoint->ballSpeed/setpoint->pathRadius;
    double spiralRadius=(0.5+0.5*sin(tau/5))*setpoint->pathRadius;
    cvZero( setpoint->integrationPositionErr );
    
    
    setPoint_x=spiralRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
    setPoint_y=spiralRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 1 );
    setPoint_vx=-spiralRadius*sin(tau);
    setPoint_vy=spiralRadius*cos(tau);
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    return;
}

void figure_eightProximity( SetPoint *setpoint, double runTime ) {
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    double tau=runTime*setpoint->ballSpeed/setpoint->pathRadius;
    cvZero( setpoint->integrationPositionErr );
    
    setPoint_x=setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 0 );
    setPoint_vx=setpoint->pathRadius*cos(tau);
    
    cvmSet( setpoint->setPoint, 0, 0, setpoint->pathRadius*sin(tau)+cvmGet( setpoint->tableCenter, 0, 0 ) );
    if (sin(tau/2) > 0){
        setPoint_y=-setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 1 )+setpoint->pathRadius;
        setPoint_vy=setpoint->pathRadius*sin(tau);
//        cvmSet( *setPoint, 2, 0, -pathRadius*cos(tau)+cvmGet( tableCenter, 0, 1 )+pathRadius);
    }
    else{
        setPoint_y=setpoint->pathRadius*cos(tau)+cvmGet( setpoint->tableCenter, 0, 1 )-setpoint->pathRadius;
        setPoint_vy=-setpoint->pathRadius*sin(tau);
//        cvmSet( *setPoint, 2, 0, pathRadius*cos(tau)+cvmGet( tableCenter, 0, 1 )-pathRadius);
    }
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    return;
}

void customProximity( SetPoint *setpoint, MaskObject ball, CvMat *trajectoryCoords) {
    double previousPoint_x, previousPoint_y, currentPoint_x, currentPoint_y;
    double nextPoint_x, nextPoint_y, nextPoint_vx, nextPoint_vy;
    double setPoint_x, setPoint_y, setPoint_vx, setPoint_vy;
    int length=trajectoryCoords->rows;
    int previousStep;
    
    currentPoint_x=ball.pixCoords[0];
    currentPoint_y=ball.pixCoords[1];
    
    CvMat *distanceToNextPoint=cvCreateMat( 2, 1, CV_32F );
    CvMat *errPos=cvCreateMat( 2, 1, CV_32F );
    CvMat *errVel=cvCreateMat( 2, 1, CV_32F );
    
    cvmSet( errPos, 0, 0, cvmGet(ball.state, 0, 0) - cvmGet(setpoint->setPointR, 0, 0) );
    cvmSet( errPos, 1, 0, cvmGet(ball.state, 2, 0) - cvmGet(setpoint->setPointR, 2, 0) );
    cvmSet( errVel, 0, 0, cvmGet(ball.state, 1, 0) - cvmGet(setpoint->setPointR, 1, 0) );
    cvmSet( errVel, 1, 0, cvmGet(ball.state, 3, 0) - cvmGet(setpoint->setPointR, 3, 0) );
    
    cvmSet( distanceToNextPoint, 0, 0, cvmGet(ball.state, 0, 0) - cvmGet( trajectoryCoords, setpoint->trajectoryStep, 0 )/calibration );
    cvmSet( distanceToNextPoint, 1, 0, cvmGet(ball.state, 2, 0) - cvmGet( trajectoryCoords, setpoint->trajectoryStep, 1 )/calibration );
    
    if(cvNorm(distanceToNextPoint) < .01 /* Closer than 1 cm*/ ){
        if( setpoint->trajectoryStep < length-1 ) //IF statement wraps trajectory around to beginning.
            setpoint->trajectoryStep++;
        else
            setpoint->trajectoryStep=0;
        
    }
    else if(cvNorm(errPos) < .03 /* Closer than 3 cm*/ ){
        
    }
    
    else{ // Too far from point. Use only coordinates as setpoint.
        
    }
    if (setpoint->trajectoryStep==0)
        previousStep=length-1;
    else
        previousStep=setpoint->trajectoryStep-1;
    
    nextPoint_x=cvmGet( trajectoryCoords, setpoint->trajectoryStep, 0 );
    nextPoint_y=cvmGet( trajectoryCoords, setpoint->trajectoryStep, 1 );
    
    nextPoint_vx=(nextPoint_x-currentPoint_x)/delT;
    nextPoint_vy=(nextPoint_y-currentPoint_y)/delT;
    
    double calculatedSpeed=sqrt(nextPoint_vx*nextPoint_vx+nextPoint_vy*nextPoint_vy);
    
//    printf("\nnextPoint_vx = %f, nextPoint_vy = %f \n", nextPoint_vx, nextPoint_vy);
//    printf("setpoint->ballSpeed = %f, calculatedSpeed = %f \n", setpoint->ballSpeed, calculatedSpeed);
    if( calculatedSpeed > setpoint->ballSpeed){
        double scaleSpeed=setpoint->ballSpeed/calculatedSpeed;
        setPoint_x=(nextPoint_x-currentPoint_x)*scaleSpeed+currentPoint_x;
        setPoint_y=(nextPoint_y-currentPoint_y)*scaleSpeed+currentPoint_y;
        setPoint_vx=nextPoint_vx*scaleSpeed;
        setPoint_vy=nextPoint_vy*scaleSpeed;
    }
    else{
        setPoint_x=nextPoint_x;
        setPoint_y=nextPoint_y;
        setPoint_vx=nextPoint_vx;
        setPoint_vy=nextPoint_vy;
    }
    
    previousPoint_x=cvmGet( trajectoryCoords, previousStep, 0 );
    previousPoint_y=cvmGet( trajectoryCoords, previousStep, 1 );
    
    double s=((ball.pixCoords[0]-nextPoint_x)*(previousPoint_x-nextPoint_x)+(ball.pixCoords[1]-nextPoint_y)*(previousPoint_y-nextPoint_y))/(pow(previousPoint_x-nextPoint_x, 2)+pow(previousPoint_y-nextPoint_y, 2));
    setPoint_x=(previousPoint_x-nextPoint_x)*s+nextPoint_x;
    setPoint_y=(previousPoint_y-nextPoint_y)*s+nextPoint_y;
    
//    printf("\ncurrentPoint_y = %f, nextPoint_y = %f \n", currentPoint_y, nextPoint_y);
//    printf("\nsetPoint_x = %f, setPoint_y = %f \n", setPoint_x, setPoint_y);

    
//    cvmSet( setpoint->setPoint, 0, 0, setPoint_x+cvmGet( setpoint->tableCenter, 0, 0 ) );
//    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
//    cvmSet( setpoint->setPoint, 2, 0, setPoint_y+cvmGet( setpoint->tableCenter, 0, 1 ) );
//    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    cvmSet( setpoint->setPoint, 0, 0, setPoint_x );
    cvmSet( setpoint->setPoint, 1, 0, setPoint_vx );
    cvmSet( setpoint->setPoint, 2, 0, setPoint_y );
    cvmSet( setpoint->setPoint, 3, 0, setPoint_vy );
    
    
    cvAdd(setpoint->integrationPositionErr, errPos, setpoint->integrationPositionErr);
    cvAdd(setpoint->integrationVelocityErr, errVel, setpoint->integrationVelocityErr);
    
    cvReleaseMat( &errPos );
    cvReleaseMat( &errVel );
    cvReleaseMat( &distanceToNextPoint );
    return;
}
